# PyQt的UI
from UI.MainWindow import Ui_MainWindow
import sys
from PyQt6.QtWidgets import QApplication,QDialog,QMainWindow
from PyQt6.QtCore import pyqtSignal,pyqtSlot
# TDOA算法和麦克风驱动库
from Drivers.AudioCapture import AudioCaptureDevice
from Algorithm import NCC,TDOA,GCC,Filter,KalmanFilter2D
# 电机驱动库
from Drivers.FOCMotorController import FOCMotorController
import threading
import numpy as np
import time
import math
import cv2 as cv
import warnings
from RPi import GPIO
# 忽略警告
warnings.filterwarnings('ignore')

# 参数设置
SAMPLE_RATE = 48000     # 采样率
SAMPLE_POINTS = 16384   # 采样点位
WAVE_SPEED = 340.0
MAX_RETRIES = 10
MEASURE_TIMEOUT = 5.0
MOTOR_OFFSET = 3.080    # 电机中点
LASER_GPIO_BCM = 17     # 激光控制的GPIO

# 打开音频设备并进行采集
dev = AudioCaptureDevice('hw:2,0','S16_LE',8,SAMPLE_RATE,2)
dev.start_capture()
# 打开电机驱动
motor_controller = FOCMotorController('/dev/ttyACM0')
motor_controller.enable()
# 初始化激光控制的GPIO
GPIO.setmode(GPIO.BCM)
GPIO.setup(LASER_GPIO_BCM,GPIO.OUT)
GPIO.output(LASER_GPIO_BCM,GPIO.LOW)

# 激光给True就开，给False就关
def set_laser_status(value):
    GPIO.output(LASER_GPIO_BCM,(GPIO.HIGH if value else GPIO.LOW))

def get_segment():
    p1_channel,p2_channel,p3_channel = dev.get_audio_segment(0.5,(6,3,0))
    # p1_channel = Filter.bandpass_filter(p1_channel,SAMPLE_RATE,FL,FH)
    # p2_channel = Filter.bandpass_filter(p2_channel,SAMPLE_RATE,FL,FH)
    # p3_channel = Filter.bandpass_filter(p3_channel,SAMPLE_RATE,FL,FH)
    p1_channel = p1_channel[:SAMPLE_POINTS] / 128
    p2_channel = p2_channel[:SAMPLE_POINTS] / 128
    p3_channel = p3_channel[:SAMPLE_POINTS] / 128
    return p1_channel,p2_channel,p3_channel

def get_location(p1_channel,p2_channel,p3_channel):
    # 对整个信号做切片其实是最好的办法
    p21_corr_gcc = np.abs(GCC.gcc_phat(p2_channel,p1_channel))
    p31_corr_gcc = np.abs(GCC.gcc_phat(p3_channel,p1_channel))
    p21_corr_valid = p21_corr_gcc[SAMPLE_POINTS//2-60:SAMPLE_POINTS//2+60]
    p31_corr_valid = p31_corr_gcc[SAMPLE_POINTS//2-60:SAMPLE_POINTS//2+60]
    r21,r31 = (p21_corr_valid.argmax()-60)/SAMPLE_RATE*WAVE_SPEED,(p31_corr_valid.argmax()-60)/SAMPLE_RATE*WAVE_SPEED
    # print(r21,r31)
    distance,anchor_tag = TDOA.chans_method([0,-0],[-0.5,0],[0.5,0],r21,r31)
    # print(anchor_tag)
    # 手动加点偏置，不然误差有点大
    # anchor_tag[0] *= 1.10
    # anchor_tag[1] *= 1.10
    # 还有另一种骚操作，就是角度求出来之后求平均做矫正
    tan_value = anchor_tag[0] / anchor_tag[1]
    corr = [2.75*tan_value,2.75]
    corrected_anchor = np.array([(corr[0] + anchor_tag[0])/2.0,(corr[1] + anchor_tag[1])/2.0])
    return distance,corrected_anchor

def check_result_is_valid(distance,anchor_tag):
    # 检查距离与坐标是否为有效值
    # 若无效就直接跳过
    if distance <= 0.0 or np.isnan(distance): return False
    if anchor_tag[0] <= -1.6 or anchor_tag[0] >= 1.6 or np.isnan(anchor_tag[0]): return False
    if anchor_tag[1] <= 2.0 or anchor_tag[1] >= 3.1 or np.isnan(anchor_tag[1]): return False
    return True

def calc_distance(p1,p2):
    return ((p2[0]-p1[0])**2 + (p2[1]-p1[1])**2)**0.5

# 基于音频相关性的TDOA系统
class TDOAMeasureSystem(object):
    STATUS_IDLE = 0     # 空闲状态
    STATUS_BUSY = 1     # 忙状态
    STATUS_TRACKING = 2 # 跟踪状态
    def __init__(self):
        self.result = None
        self.status = self.STATUS_IDLE
        self.thread = None
        self.thread_lock = threading.Lock()
    def print_log(self,text):
        pass
    def clear_log(self):
        pass
    # 单次测量过程
    def single_measurement_process(self,cycle_mode = False):
        # 指示结果是否有效且稳定
        result_valid = False
        latest_results = []
        t0 = time.time()
        final_point = [0,0]
        while not result_valid:
            ch1,ch2,ch3 = get_segment()
            _distance,anchor_tag = get_location(ch1,ch2,ch3)
            print(anchor_tag)
            curr_result_valid = check_result_is_valid(_distance,anchor_tag)
            # 若测量到3次有效结果且相对稳定则取这个结果
            if curr_result_valid:
                if len(latest_results) < 3:
                    latest_results.append(anchor_tag)
                else:
                    center,radius = cv.minEnclosingCircle(np.array(latest_results,dtype=np.float32))
                    if radius < 0.20: # 外接圆半径小于0.15
                        result_valid = True
                        final_point = center
                        break
                    else:
                        # 否则就继续循环加入列表
                        latest_results.pop(0)
                        latest_results.append(anchor_tag)
            # 超时退出
            if time.time() - t0 >= MEASURE_TIMEOUT:
                break
        # 这里设置循环模式是为了更好的复用代码
        if not cycle_mode:
            self.set_status(self.STATUS_IDLE)
        self.print_log('\n=========================')
        if not result_valid:
            self.print_log('测量超时，未得到有效值.')
            self.result = False,final_point,0,0
            return
        final_distance = calc_distance(final_point,[0,0])
        final_angle = math.atan2(final_point[1],final_point[0])/(math.pi*2)*360.0 - 90
        self.print_log('测量完毕，测量结果如下：\n - 坐标:X=%0.3f,Y=%0.3f\n - 角度=%0.3fdeg\n - 距离:%0.3fm\n'%(
            final_point[0],
            final_point[1],
            final_angle,
            final_distance,
            ))
        self.result = result_valid,final_point,final_angle,final_distance
    # 单次追踪过程
    def single_tracking_process(self):
        # 之前那一堆代码可以不用了，复用一下单次测量的代码
        self.single_measurement_process()
        is_valid,point,angle,final_distance = self.result
        if is_valid:
            # 发送电机指令
            set_laser_status(True)
            # 之前忘记算电机的位置了，这里加上去，因为电机在原点后0.3m
            motor_angle = math.atan2(point[1]+0.3,point[0]) - math.pi/2
            motor_controller.set_angle(motor_angle + MOTOR_OFFSET)
            self.print_log('电机指令发送完毕.')
    # 连续追踪过程
    def continous_tracking_process(self):
        prev_angle = 0
        while self.status == self.STATUS_TRACKING:
            # 这里就开启循环模式，当最后关闭的时候才释放状态位
            self.single_measurement_process(True)
            is_valid,point,angle,final_distance = self.result
            if is_valid:
                # 发送电机指令
                motor_angle = math.atan2(point[1]+0.3,point[0]) - math.pi/2
                # 添加简单的滤波
                motor_angle = (prev_angle + motor_angle) / 2
                prev_angle = motor_angle
                motor_controller.set_angle(motor_angle + MOTOR_OFFSET)
                self.print_log('电机指令发送完毕.')
            # 暂时就加个延迟比较好
            time.sleep(0.1)
        self.set_status(self.STATUS_IDLE)
    def start_single_measurement(self):
        self.set_status(self.STATUS_BUSY)
        set_laser_status(False)
        self.thread = threading.Thread(target=self.single_measurement_process)
        self.thread.start()
        self.print_log('正在进行单次测量......')
    def start_single_tracking(self):
        self.set_status(self.STATUS_BUSY)
        # 刚开始时激光必须处于关闭状态
        set_laser_status(False)
        self.thread = threading.Thread(target=self.single_tracking_process)
        self.thread.start()
        self.print_log('正在进行单次追踪......')
    def start_continous_tracking(self):
        self.set_status(self.STATUS_TRACKING)
        set_laser_status(True)
        self.thread = threading.Thread(target=self.continous_tracking_process)
        self.thread.start()
        self.print_log('正在进行连续追踪......')
    def stop_continous_tracking(self):
        set_laser_status(False)
        self.set_status(self.STATUS_IDLE)
        self.thread.join()
        self.print_log('停止连续追踪模式.')
    def get_status(self):
        return self.status
    # 为了线程安全加锁操作
    def set_status(self,status):
        self.thread_lock.acquire()
        self.status = status
        self.thread_lock.release()

# 由PyUIC生成的UI
class MainWindow(QMainWindow):
    print_log_signal = pyqtSignal(str,name='print_log_signal')
    clear_log_signal = pyqtSignal(name='clear_log_signal')
    def __init__(self,parent = None):
        super().__init__(parent)
        self.ui = Ui_MainWindow()
        # 初始化UI
        self.ui.setupUi(self)
        self.showFullScreen()
        self.logging_text = ''
        self.measure_system = TDOAMeasureSystem()
        self.measure_system.print_log = self.print_log
        self.measure_system.clear_log = self.clear_log
        self.print_log_signal.connect(self.print_log_slot)
        self.clear_log_signal.connect(self.clear_log_slot)
    # 日志信息的输出
    def print_log(self,text):
        self.print_log_signal.emit(text)
    def clear_log(self):
        self.clear_log_signal.emit()
    # 日志相关的槽函数需要自行编写
    @pyqtSlot(str)
    def print_log_slot(self,text):
        self.ui.plainTextEdit.appendPlainText(text)
    @pyqtSlot()
    def clear_log_slot(self):
        self.ui.plainTextEdit.clear()
    # 这里按键相关的槽函数按UI对象的名称写就行
    def on_pushButton_pressed(self):
        # 单次测量的按钮
        if self.measure_system.get_status() != self.measure_system.STATUS_IDLE:
            self.print_log('测量系统忙，请在当次测量完毕后再继续进行')
            return
        self.measure_system.start_single_measurement()
    def on_pushButton_2_pressed(self):
        # 单次测量的按钮
        if self.measure_system.get_status() != self.measure_system.STATUS_IDLE:
            self.print_log('测量系统忙，请在当次测量完毕后再继续进行')
            return
        self.measure_system.start_single_tracking()
    def on_pushButton_3_pressed(self):
        if self.measure_system.get_status() != self.measure_system.STATUS_IDLE:
            self.print_log('测量系统忙，请在当次测量完毕后再继续进行')
            return
        self.measure_system.start_continous_tracking()
    def on_pushButton_4_pressed(self):
        self.measure_system.stop_continous_tracking()
    
        
        

if __name__ == "__main__":
    app = QApplication(sys.argv)
    window = MainWindow()
    window.show()
    sys.exit(app.exec())
    